//
// Created by nuo on 2021/11/18.
//

#ifndef SRC_SERIAL_TEST_H
#define SRC_SERIAL_TEST_H

#include "../mmuse.h"  //使用这个头文件中的函数，会有multiple definition of *** 的错误，暂时把函数写在class中


#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Dense>
#include <std_msgs/Float32MultiArray.h>
#include <yaml-cpp/yaml.h>
#include "global_defination/global_defination.h"

//typedef union {
//    unsigned char cvalue[4];
//    double fvalue;
//}double_union;

namespace serial_test
{
    class SerialTest{

    public:
        SerialTest(ros::NodeHandle &nh);

        ~SerialTest()= default;

        void write_callback(const std_msgs::String::ConstPtr& msg);

        void cmd_twist_callback(const geometry_msgs::TwistConstPtr & cmd_twist_msg);

        void stc_sub_callback(const std_msgs::Float32MultiArrayConstPtr& std_sub_msg);

        void allocateMemory();

        void float2bytes(float p,unsigned char *bytes);
        float bytes2float(const unsigned char *bytes);
        Eigen::Quaternionf Euler2Quaternionf(float eluer_z, float eluer_y, float eluer_x);

        bool serial_opened();
        void read_test();
        void send_odom_stc();




        void Run();


    private:
        serial::Serial ser; //声明串口数据

        ros::Subscriber write_sub;  // 测试案例
        ros::Publisher read_pub;  // 测试案例

        ros::Subscriber cmd_vel_sub;  // 获得 /cmd_vel话题名上的控制指令
        ros::Subscriber odom_stc_sub;  // 获得 stc单片机上传的数据
        ros::Publisher odom_stc_pub;  // 发布从stc单片机上获得的数据

//        std::uint8_t cmd_twist_buffer[17];  // cmd_vel上的数据转换成byte数据的存储容器
//        std::uint8_t odom_stc_buffer[29];   // 从stc单片机上获得的byte数据的存储容器
        // cmd控制指令解析
        double_union velocity_x,velocity_y,angular_v;
        // stc单片机上传数据解析 成 float ，并发布为 /odom_stc 话题
        //double_union position_x, position_y, liner_x, liner_y, angular_speed_z, angular_pose_z;
        double_union odom_stc_array[6];
        nav_msgs::Odometry odom_stc_data;

        YAML::Node config_node_;
        int num_byte_to_stc, num_byte_from_stc;
    };

}











#endif //SRC_SERIAL_TEST_H
